
#include "ContactSolver.h"
#include "World.h"

namespace p2 {

ContactSolver::~ContactSolver()
{
}

void ContactSolver::solve(real64 dT, Contact::List& contacts, uint32 numIterations)
{
	for (uint32 j = 0; j < numIterations; j++) {
		Contact::List::iterator iter = contacts.begin();
		for (; iter != contacts.end(); ++iter) {
			Contact& contact = (*iter);
            Entity* ea = contact._a->getOwner();
            Entity* eb = contact._b->getOwner();

            real muK = p2Material::combineFriction(contact._a->getMaterial(), contact._b->getMaterial());
			for (uint32 pi = 0; pi < contact._nppb; ++pi) {
				contact._dv[pi] = eb->getV() + contact._rb[pi] * eb->getW() - ea->getV() - contact._ra[pi] * ea->getW();
			}
			for (uint32 pi = 0; pi < contact._nppb; ++pi) { // contact._nppb <= 2
				/*
				 * compute total relative velocity for collision normal and tangent
				 */
				real relNv = contact._dv[pi].dot(contact._normal);
				Vector2<real> tangent = contact._normal.perp();
				real relTV = contact._dv[pi].dot(tangent);
                /*
                 * compute and apply normal impulse; remove only the amount
                 * which leaves contacts touching next frame
                 */
                real remove = relNv + contact._dist[pi] / (real) dT;
                real mag = remove * contact._invDenom[pi];
                /*
                 * remove only the amount which leaves them touching next frame
                 */
                real newImpulse = Math<real>::Min(0.f, contact._impulseN[pi] + mag);
                real change = newImpulse - contact._impulseN[pi];

                Vector2<real> imp = contact._normal * change;

                ea->incV(+ea->getInvMass() * imp);
                ea->incW(+ea->getInvInertia() * imp.dot(contact._ra[pi]));
                eb->incV(-eb->getInvMass() * imp);
                eb->incW(-eb->getInvInertia() * imp.dot(contact._rb[pi]));

                //contact.applyImpulses(imp, pi);
                contact._impulseN[pi] = newImpulse;

                /*
                 * compute friction as a function of normal impulse
                 */
                real absMag = Math<real>::FAbs(contact._impulseN[pi]) * muK;

                /*
                 * compute and apply tangental impulse
                 */
                mag = relTV * contact._invDenomTan[pi];
                newImpulse = Math<real>::Clamp(contact._impulseT[pi] + mag, -absMag, absMag);
                change = newImpulse - contact._impulseT[pi];
                imp = tangent * change;

                ea->incV(+ea->getInvMass() * imp);
                ea->incW(+ea->getInvInertia() * imp.dot(contact._ra[pi]));
                eb->incV(-eb->getInvMass() * imp);
                eb->incW(-eb->getInvInertia() * imp.dot(contact._rb[pi]));

                //contact.applyImpulses(imp, pi);
                contact._impulseT[pi] = newImpulse;
            }
		}
	}
}

}
